//
// Created by enemy1205 on 2021/9/16.
//
/////用于放置一些遗弃的代码//////
//void ArmorDetect::Predict() {
//    //auto time_it = time_sequence.cbegin();
//    auto it = Armors_sequence.cbegin();
//    if (it->empty() || next(it)->empty() || next(it, 2)->empty() || next(it, 3)->empty()) return;
//    auto goal_armor1 = *(it->cbegin());
//    auto goal_armor2 = *(next(it, 1)->cbegin());
//    auto goal_armor3 = *(next(it, 2)->cbegin());
//    auto goal_armor4 = *(next(it, 3)->cbegin());
//    float x_mvSpeed1 = (goal_armor1->center.x - goal_armor2->center.x) / time_sequence[0];
//    float x_mvSpeed2 = (goal_armor2->center.x - goal_armor3->center.x) / time_sequence[1];
//    float x_mvSpeed3 = (goal_armor3->center.x - goal_armor4->center.x) / time_sequence[2];
//    float y_mvSpeed1 = (goal_armor1->center.y - goal_armor2->center.y) / time_sequence[0];
//    float y_mvSpeed2 = (goal_armor2->center.y - goal_armor3->center.y) / time_sequence[1];
//    float y_mvSpeed3 = (goal_armor3->center.y - goal_armor4->center.y) / time_sequence[2];
//    //        cout<<x_mvSpeed1<<"  "<<time_sequence[0]<<endl;
//    float angle_Speed1 = (goal_armor1->angle - goal_armor2->angle) / time_sequence[0];
//    float angle_Speed2 = (goal_armor2->angle - goal_armor3->angle) / time_sequence[1];
//    float angle_Speed3 = (goal_armor3->angle - goal_armor4->angle) / time_sequence[2];
//    Predicted_offset_x = (x_mvSpeed1 * Other_param.weight_v1 + x_mvSpeed2 * Other_param.weight_v2 +
//                          x_mvSpeed3 * Other_param.weight_v3) * Other_param.fly_time / Other_param.decay_k;
//    Predicted_offset_y = (y_mvSpeed1 * Other_param.weight_v1 + y_mvSpeed2 * Other_param.weight_v2 +
//                          y_mvSpeed3 * Other_param.weight_v3) * Other_param.fly_time / Other_param.decay_k;
//    Predicted_offset_angle = (angle_Speed1 * Other_param.weight_v1 + angle_Speed2 * Other_param.weight_v2 +
//                              angle_Speed3 * Other_param.weight_v3) * Other_param.fly_time / Other_param.decay_k;
//    //        cout << Predicted_offset_x << "  " << Predicted_offset_y << endl;
//    // Predicted_armor共享指针导致赋值问题,改为一般指针正常
//    Predicted_armor = new Armor(*goal_armor1);
//    Predicted_armor->angle += Predicted_offset_angle;
//    Predicted_armor->center += Point2f(Predicted_offset_x, Predicted_offset_y);
//    Predicted_armor->size = Size2f(Predicted_armor->getWidth() / 2, Predicted_armor->getHeight() / 2);//椭圆需要的size为半长和半宽
////    cout << "预测 " << Predicted_armor->center << "当前实际  " << goal_armor1->center << endl;
//}
//
//void ArmorDetect::update() {
//    if (this->get_current_armors().empty() && Predicted_armor != nullptr) {
//        this->Armors_sequence[0].emplace_back(make_shared<Armor>(*(this->Predicted_armor)));//利用预测装甲板补帧
//    } else {
//        this->Armors_sequence.emplace_front(this->get_current_armors());
////    根据距离最近选取目标装甲板并排到首位
////        sort(this->get_current_armors().begin(), this->get_current_armors().end(),
////             [&](Armor_ptr &armor1, Armor_ptr &armor2) -> bool { return armor1->pnp_distance < armor2->pnp_distance; });
////        this->goalArmor = this->get_current_armors()[0];
//    }
//    this->time_sequence.emplace_front(this->interval_time);
//    //        log.INFO(to_string(trackers->interval_time)+"s");
//    if (time_sequence.size() > Other_param.t_Seq_maxlength) time_sequence.pop_back();
//    if (Armors_sequence.size() > A_param.Seq_maxlength) Armors_sequence.pop_back();
//    //保证有足够有效帧预测
//    if (this->Armors_sequence.size() > 3 && !get_current_armors().empty()) {
//        Predict();
//    }
//}
//
/*void Match_detect(const vector<Vec4i> &hierarchy,const vector<vector<Point>> &contours,Mat &dst){
    //用于记录扇叶编号，方便保存图片
    int cnnt = 0;
    Mat template_img[9];
    for (int i = 1; i <= 8; i++) {
        template_img[i] = imread("/home/enemy1205/下载/Demo-master/template" + to_string(i) + ".jpg",
                                 IMREAD_GRAYSCALE);
    }
    //遍历轮廓
    RotatedRect rect_tmp2;
    if (hierarchy.size())
        for (int i = 0; i >= 0; i = hierarchy[i][0]) {
            rect_tmp2 = minAreaRect(contours[i]);
            Point2f P[4];
            rect_tmp2.points(P);

            Point2f srcRect[4];
            Point2f dstRect[4];
            double width;
            double height;
            //矫正提取的叶片的宽高
            width = getDistance(P[0], P[1]);
            height = getDistance(P[1], P[2]);
            if (width > height) {
                srcRect[0] = P[0];
                srcRect[1] = P[1];
                srcRect[2] = P[2];
                srcRect[3] = P[3];
            } else {
                swap(width, height);
                srcRect[0] = P[1];
                srcRect[1] = P[2];
                srcRect[2] = P[3];
                srcRect[3] = P[0];
            }
            double area = height * width;
            if (area > 1000) {
                dstRect[0] = Point2f(0, 0);
                dstRect[1] = Point2f(width, 0);
                dstRect[2] = Point2f(width, height);
                dstRect[3] = Point2f(0, height);
                // 应用透视变换，矫正成规则矩形
                Mat transform = getPerspectiveTransform(srcRect, dstRect);
                Mat perspectMat;
                warpPerspective(dst, perspectMat, transform, dst.size());
                // 提取扇叶图片
                Mat testim;
                testim = perspectMat(Rect(0,0,width,height));
                imshow("testim",testim);
            }
        }
};*/


void RuneDetect::getRealCenter() {
//    float diff_0_1 = abs(Rune_centers_sequence[0].x - Rune_centers_sequence[1].x);
//    float diff_0_2 = abs(Rune_centers_sequence[0].x - Rune_centers_sequence[2].x);
//    float diff_1_2 = abs(Rune_centers_sequence[1].x - Rune_centers_sequence[2].x);
//    for (Point2f Pt:Rune_centers_sequence) cout << Pt << "  ";
//    //0表示误识别（一般为0）或者已经离开位置  均取最新帧为准确位置
//    if (diff_0_1 < 20 && diff_0_2 < 20) //新帧位置基本不变情况:real real real
//        this->RealCenter = Rune_centers_sequence[0];
//    if (diff_0_1 < 20 && diff_0_2 > 20) {
//        if (Rune_centers_sequence[0].x == 0 || Rune_centers_sequence[1].x == 0)//防止出现连续两帧识别为空的错误情况:0 0 real
//            this->RealCenter = Rune_centers_sequence[2];
//        else this->RealCenter =Rune_centers_sequence[0];
//    }//位置移动后第三帧偏差较大情况:real real 0
//    if (diff_0_1 > 20 && diff_0_2 < 20) {
//        if (Rune_centers_sequence[0].x == 0 || Rune_centers_sequence[2].x == 0)//防止出现0 real 0情况
//            this->RealCenter = Rune_centers_sequence[1];
//        else this->RealCenter = Rune_centers_sequence[0];
//    }//误识别帧随队列推移来到第二帧:real 0 real
//    if (diff_0_1 > 20 && diff_0_2 > 20) {
////        if (diff_1_2 < 20)
////            this->RealCenter = Rune_centers_sequence[1];//新帧误识别情况:0 real real
//             this->RealCenter = Rune_centers_sequence[0];//位置移动后取最近帧中心 :real 0 0
//    }
//    //        sort(Rune_centers_sequence.begin(), Rune_centers_sequence.end(),
//    //             [&](Point2f &pt_1, Point2f &pt_2) -> bool {return pt_1.x > pt_2.x;});
//    cout << "计算中心" << RealCenter << endl;
    //        log.INFO("[" + to_string(Rune_centers_sequence[0].x) + "," + to_string(Rune_centers_sequence[0].y) + "]   " +
    //                 "[" + to_string(Rune_centers_sequence[1].x) + "," + to_string(Rune_centers_sequence[1].y) + "]   " +
    //                 "[" + to_string(Rune_centers_sequence[2].x) + "," + to_string(Rune_centers_sequence[2].y) + "]   " +
    //                 "计算中心: [" + to_string(RealCenter.x) + "," + to_string(RealCenter.y) + "]");
    if (Rune_centers_sequence[0].x == 0)
        if (Rune_centers_sequence[1].x == 0) RealCenter = Rune_centers_sequence[1];
        else RealCenter = Rune_centers_sequence[1];
    else RealCenter = Rune_centers_sequence[0];
    Rune_centers_sequence.pop_back();
}


//void RuneDetect::Predict() {
//    if (this->RealCenter.x - Rune_centers_sequence[1].x > 20) {
//        Predicted_R_center = R_Armors_sequence[0][0]->center;
//        return;
//    }
//    auto it = R_Armors_sequence.cbegin();
//    auto goal_armor1 = *(it->cbegin());
//    auto goal_armor2 = *(next(it, 1)->cbegin());
//    auto goal_armor3 = *(next(it, 2)->cbegin());
//    float angle1 = atan(
//            (goal_armor1->center.y - this->RealCenter.y) / (goal_armor1->center.x - this->RealCenter.x));
//    float angle2 = atan(
//            (goal_armor2->center.y - this->RealCenter.y) / (goal_armor2->center.x - this->RealCenter.x));
//    float angle3 = atan(
//            (goal_armor3->center.y - this->RealCenter.y) / (goal_armor3->center.x - this->RealCenter.x));
//    Predicted_offset_angle =
//            abs((angle1 - angle2) / time_sequence[0] * Other_param.weight_w1 +
//                (angle2 - angle3) / time_sequence[1] * Other_param.weight_w2) *
//            Other_param.fly_time;
//    //点绕点旋转,opencv坐标系下逆时针旋转
//    Predicted_R_center.x = (goal_armor1->center.x - this->RealCenter.x) * cos(Predicted_offset_angle) -
//                           (goal_armor1->center.y - this->RealCenter.y) * sin(Predicted_offset_angle) +
//                           this->RealCenter.x;
//    Predicted_R_center.y = (goal_armor1->center.y - this->RealCenter.y) * cos(Predicted_offset_angle) +
//                           (goal_armor1->center.x - this->RealCenter.x) * sin(Predicted_offset_angle) +
//                           this->RealCenter.y;
////    cout<<Predicted_offset_angle<<endl;
//    cout << "当前中心:" << goal_armor1->center << "  " << "预测中心 " << Predicted_R_center << endl;
//}


//
//void RuneDetect::update() {
//    if (this->currentRune->Rune_armor().empty()) {
//        this->R_Armors_sequence.clear();
//    } else this->R_Armors_sequence.emplace_front(this->currentRune->Rune_armor());
////    this->R_Armors_sequence.emplace_front(this->currentRune->Rune_armor());
//    this->time_sequence.emplace_front(this->interval_time);
//    if (time_sequence.size() > Other_param.t_Seq_maxlength) time_sequence.pop_back();
//    if (R_Armors_sequence.size() > R_param.Seq_maxlength) R_Armors_sequence.pop_back();
//    if (Rune_centers_sequence.size() > 2) {
//        this->getRealCenter();
//    }
//    if (this->R_Armors_sequence.size() > 2) {
//        Predict();
//    }
//}

//Pnp距离移交至Tracker部分
void Armor::get_Pnp_Dis() {
    vector<Point2f> img_points;
    for (auto & vertexe : this->vertexes) {
        img_points.push_back(vertexe);
    }
    Mat rvec;              // 旋转向量
    Mat tvec;              // 平移向量
    solvePnP(A_param.Small_Armor, img_points, Other_param.cameraMatrix,
             Other_param.distCoeff, rvec, tvec,
             false, SOLVEPNP_IPPE);
    //利用at直接访问tvec的效率较低
    tvec.convertTo(tvec, CV_32FC1);
    Eigen::Vector3d transfer_vector;
    cv2eigen(tvec, transfer_vector);
    this->pnp_distance = sqrt(transfer_vector.transpose() * transfer_vector);
}

